/**
 *@note Copyright 2012-2022 CETHIK All Rights Reserved.
 *@brief 数据处理类文件
 *@author tuyinan
 *@data 2022-04-xx
 */
#include <costmap_2d/observation_buffer.h>

#include <iostream>

using namespace std;

namespace costmap_2d {
ObservationBuffer::ObservationBuffer(ConfigFile* configNode) {
  configNode_ = configNode;
  observation_keep_time_ =
      (*configNode_).getValueOfKey<double>("observation_keep_time", 1.0);
  expected_update_rate_ =
      (*configNode_).getValueOfKey<double>("expected_update_rate", 0.2);
  last_updated_ = time(0);
  min_obstacle_height_ =
      (*configNode_).getValueOfKey<double>("min_obstacle_height", 0.2);
  max_obstacle_height_ =
      (*configNode_).getValueOfKey<double>("max_obstacle_height", 1.0);
  obstacle_range_ = (*configNode_).getValueOfKey<double>("obstacle_range", 1.0);
  raytrace_range_ = (*configNode_).getValueOfKey<double>("raytrace_range", 1.0);
}

ObservationBuffer::~ObservationBuffer() {}

// cloud 处理方法，cloud坐标系转换处理
void ObservationBuffer::bufferCloud(
    costmap_2d::PointCloud* cloud,
    Eigen::Transform<float, 3, Eigen::Isometry> transform,
    costmap_2d::Point origin) {
  // create a new observation on the list to be populated
  doTransform(cloud, transform);
  observation_list_.push_front(Observation());

  // make sure to pass on the raytrace/obstacle range of the observation buffer
  // to the observations
  observation_list_.front().raytrace_range_ = raytrace_range_;
  observation_list_.front().obstacle_range_ = obstacle_range_;
  for (int i = 0; i < cloud->points.size(); i++) {
    if (cloud->points[i].z >= max_obstacle_height_ ||
        cloud->points[i].z <= min_obstacle_height_) {
      cloud->points.erase(cloud->points.begin() + i);
      i--;
    }
  }
  observation_list_.front().cloud_ = cloud;
  observation_list_.front().origin_ = origin;
  // if the update was successful, we want to update the last updated time
  last_updated_ = time(0);

  // we'll also remove any stale observations from the list
  purgeStaleObservations();
}

// returns a copy of the observations
void ObservationBuffer::getObservations(vector<Observation>& observations) {
  // first... let's make sure that we don't have any stale observations
  purgeStaleObservations();

  // now we'll just copy the observations for the caller
  list<Observation>::iterator obs_it;
  for (obs_it = observation_list_.begin(); obs_it != observation_list_.end();
       ++obs_it) {
    observations.push_back(*obs_it);
  }
}
// 消除过时信息
void ObservationBuffer::purgeStaleObservations() {
  if (!observation_list_.empty()) {
    list<Observation>::iterator obs_it = observation_list_.begin();
    //  设置有效的时间
    // if we're keeping observations for no time... then we'll only keep one
    // observation
    if (observation_keep_time_ == 0.0) {
      observation_list_.erase(++obs_it, observation_list_.end());
      return;
    }

    // otherwise... we'll have to loop through the observations to see which
    // ones are stale
    for (obs_it = observation_list_.begin(); obs_it != observation_list_.end();
         ++obs_it) {
      Observation& obs = *obs_it;
      // check if the observation is out of date... and if it is, remove it and
      // those that follow from the list
      if (difftime(last_updated_, obs.cloud_->time) > observation_keep_time_) {
        observation_list_.erase(obs_it, observation_list_.end());
        return;
      }
    }
  }
}

bool ObservationBuffer::isCurrent() const {
  if (expected_update_rate_ == 0.0) {
    return true;
  }

  bool current = (difftime(time(0), last_updated_) <= expected_update_rate_);
  // if (!current){
  //   std::cout<<"observation rate too slow!!!"<<std::endl;
  // }
  return current;
}

void ObservationBuffer::resetLastUpdated() { last_updated_ = time(0); }
}  // namespace costmap_2d
